#include "custom_nav_controller/custom_nav_controller.hpp"

#include "nav2_util/node_utils.hpp"

#include <iostream>
#include <vector>

namespace custom_nav_controller
{
  CustNavController::CustNavController() {}

  CustNavController::~CustNavController() {}

/**
 * @param parent 指向Node节点的指针
 * @param costmap_ros 代价地图指针
 */
void CustNavController::configure(
    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
    std::string name,
    std::shared_ptr<tf2_ros::Buffer> tf,
    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
  node_ = parent.lock();
  costmap_ros_ = costmap_ros;

  plugin_name_ = name;

  RCLCPP_INFO(node_->get_logger(), "Configuring CustNavController: %s", name.c_str());

   // 声明并获取参数，设置最大线速度和最大角速度
  nav2_util::declare_parameter_if_not_declared(
      node_, plugin_name_ + ".max_linear_speed", rclcpp::ParameterValue(1.0));
  node_->get_parameter(plugin_name_ + ".max_linear_speed", max_lin_speed_);
  nav2_util::declare_parameter_if_not_declared(
      node_, plugin_name_ + ".max_angular_speed", rclcpp::ParameterValue(1.0));
  node_->get_parameter(plugin_name_ + ".max_angular_speed", max_ang_speed_);
}


/**
 * @brief 激活控制器与执行任何相关线程
 * 
 */
void CustNavController::activate()
{
  RCLCPP_INFO(node_->get_logger(), "Activating CustNavController");
}

/**
 * @brief 与activate函数相反
 * 
 */
void CustNavController::deactivate()
{
  RCLCPP_INFO(node_->get_logger(), "Deactivating CustNavController");
}

/**
 * @brief 清理资源
 */
void CustNavController::cleanup()
{
  RCLCPP_INFO(node_->get_logger(), "Cleaning up CustNavController");
}


/**
 * @brief 设置全局路径
 * 
 * @param path 全局路径
 * @return * void 
 */
void CustNavController::setPlan(const nav_msgs::msg::Path & path)
{
  global_path_ = path;
  RCLCPP_INFO(node_->get_logger(), "Received global path with %zu points", path.poses.size());
}


/**
 * @brief 根据当前pose和vel 计算最佳命令
 * 
 * @param pose 当前robot姿态
 * @param velocity 当前速度
 * @param goal_checker 任务正在使用的目标`检查器`指针
 * @return geometry_msgs::msg::TwistStamped 
 */
geometry_msgs::msg::TwistStamped CustNavController::computeVelocityCommands(
    const geometry_msgs::msg::PoseStamped & pose,
    const geometry_msgs::msg::Twist & velocity,
    nav2_core::GoalChecker * goal_checker)
{
   // 1. 检查路径是否为空
  if (global_path_.poses.empty()) {

#if defined(ROS_DISTRO_JAZZY)  // Jazzy
  throw nav2_core::ControllerException("收到长度为零的路径");
#else                          // Humble
    throw nav2_core::PlannerException("收到长度为零的路径");
#endif

  }

  // 2.将机器人当前姿态转换到全局计划坐标系中
geometry_msgs::msg::PoseStamped pose_in_globalframe;
      // transformPoseInTargetFrame 将当前pose转成世界坐标, 0.1 超时时间
  if (!nav2_util::transformPoseInTargetFrame(
          pose, pose_in_globalframe, *tf_, global_path_.header.frame_id, 0.1)) {

#if defined(ROS_DISTRO_JAZZY)  // Jazzy
  throw nav2_core::ControllerException("无法将机器人姿态转换为全局计划的坐标系");
#else                          // Humble
  throw nav2_core::PlannerException("无法将机器人姿态转换为全局计划的坐标系");
#endif

  }

  // 3.获取最近的目标点和计算角度差
  auto target_pose = getNearestTargetPose(pose_in_globalframe);
  auto angle_diff = calcAngDiff(pose_in_globalframe, target_pose);

  // 4.根据角度差计算线速度和角速度
  geometry_msgs::msg::TwistStamped cmd_vel;
  cmd_vel.header.frame_id = pose_in_globalframe.header.frame_id;
  cmd_vel.header.stamp = node_->get_clock()->now();

  // 根据角度差计算速度，角度差大于 0.3 则原地旋转，否则直行
  if (fabs(angle_diff) > M_PI/10.0) {
    cmd_vel.twist.linear.x = .0;
    cmd_vel.twist.angular.z = fabs(angle_diff) / angle_diff * max_ang_speed_;
  } else {
    cmd_vel.twist.linear.x = max_lin_speed_;
    cmd_vel.twist.angular.z = .0;
  }

  RCLCPP_INFO(node_->get_logger(), "控制器：%s 发送速度(%f,%f)",
              plugin_name_.c_str(), cmd_vel.twist.linear.x,
              cmd_vel.twist.angular.z);
  
  return cmd_vel;
}

/**
 * @brief 限制最大线性速度
 * 
 * @param speed_limit 以绝对值表示的速度（单位：米）或是最大机器人速度的百分比
 * @param percentage 如果为true以百分比表示
 */
void CustNavController::setSpeedLimit(const double & speed_limit, const bool & percentage)
{
  RCLCPP_INFO(node_->get_logger(), "Setting speed limit: %.2f (percentage: %s)", 
              speed_limit, percentage ? "true" : "false");
}

/**
 * @brief 获取路径中距离当前点最近的点
 * 
 * @param current_pose 
 * @return geometry_msgs::msg::PoseStamped
 */
geometry_msgs::msg::PoseStamped CustNavController::getNearestTargetPose(const geometry_msgs::msg::PoseStamped &current_pose)
{
  using nav2_util::geometry_utils::euclidean_distance;

  // 1. 获取距离最近的全局节点索引

  int nearest_pose_index = 0;

  // 计算距离
  double min_dist = euclidean_distance(current_pose, global_path_.poses.at(0), false);

  // 遍历是否存在小于当前距离，置换并替代
  for (unsigned int i = 1; i < global_path_.poses.size(); i++) {
    double dist = euclidean_distance(current_pose, global_path_.poses.at(i));
    if (dist < min_dist) {
      nearest_pose_index = i;
      min_dist = dist;
    }
  }

  // 2.从路径中擦除头部到最近点的路径
  // 从0到nearest_pose_index擦除
  global_path_.poses.erase(std::begin(global_path_.poses),
                           std::begin(global_path_.poses) + nearest_pose_index);


  // 3.如果只有一个点则直接，否则返回最近点的下一个点clear
  if (global_path_.poses.size() == 1) {
    return global_path_.poses.at(0);
  }
  
  return global_path_.poses.at(1);
}

/**
 * @brief 计算目标点方向和当前位置的角度差
 * 
 * @param current_pose 
 * @param target_pose 
 * @return double 
 */
double calcAngDiff(const geometry_msgs::msg::PoseStamped &current_pose, const geometry_msgs::msg::PoseStamped &target_pose)
{
  // 1. 获取当前角度

  float _current_yaw = tf2::getYaw(current_pose.pose.orientation);
  
  // 2.获取目标点朝向
  float target_angle = 
          std::atan2(target_pose.pose.position.y - current_pose.pose.position.y,
                     target_pose.pose.position.x - current_pose.pose.position.x);
  
  // 3.计算角度差，并转换到 -M_PI 到 M_PI 之间
  double angle_diff = target_angle - _current_yaw;
  if (angle_diff < -M_PI) {
    angle_diff += 2.0 * M_PI;
  } else if (angle_diff > M_PI) {
    angle_diff -= 2.0 * M_PI;
  }

  return angle_diff;
}

}  // namespace custom_nav_controller
 
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(custom_nav_controller::CustNavController, nav2_core::Controller)